﻿using Newtonsoft.Json;
using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.IO;
using System.Text;
using System.Threading;

namespace DuBaiCoffee2.Model
{
    /// <summary>
    /// 机器人控制
    /// </summary>
    public class RobotOperate
    {
        public static string RobotOperateUrl = AppDomain.CurrentDomain.SetupInformation.ApplicationBase;
        public static int handle1 = 0;
        public static int handle2 = 0;
        public static double PI = 3.1415926535;
        public static List<string[]> listStrArr1 = new List<string[]>();//数组List，相当于可以无限扩大的二维数组。
        public static List<string[]> listStrArr2 = new List<string[]>();
        public static List<string[]> listStrArr3 = new List<string[]>();//数组List，相当于可以无限扩大的二维数组。
        public static List<string[]> listStrArr4 = new List<string[]>();
        public static List<string[]> listStrArr5 = new List<string[]>();//数组List，相当于可以无限扩大的二维数组。
        public static List<string[]> listStrArr6 = new List<string[]>();
        private static RobotOperate instance;
        private RobotOperate() { }
        public static RobotOperate Instance
        {
            get
            {
                if (instance == null)
                {
                    instance = new RobotOperate();
                }
                return instance;
            }
        }
        //public static JKTYPE.ProgramState pstatusR;
        //public static JKTYPE.ProgramState pstatusL;
        //public static JKTYPE.RobotState stateL;
        //public static JKTYPE.RobotStatus statusL;
        //public static JKTYPE.RobotState stateR;
        //public static JKTYPE.RobotStatus statusR;
        //public static void CX_Start_L()
        //{
        //    try
        //    {
        //        jakaAPI.get_program_state(ref handle1, ref pstatusL);
        //    }
        //    catch (Exception)
        //    {
        //        return;
        //    }
        //}
        //public static void CX_kj_L()
        //{
        //    try
        //    {
        //        jakaAPI.get_robot_state(ref handle1, ref stateL);
        //    }
        //    catch (Exception)
        //    {
        //        return;
        //    }
        //}
        //public static void CX_status_L()
        //{
        //    try
        //    {
        //        jakaAPI.get_robot_status(ref handle1, ref statusL);
        //    }
        //    catch (Exception)
        //    {
        //        return;
        //    }
        //}
        //public static void CX_Start_R()
        //{
        //    try
        //    {
        //        jakaAPI.get_program_state(ref handle2, ref pstatusR);
        //    }
        //    catch (Exception)
        //    {
        //        return;
        //    }
        //}
        //public static void CX_kj_R()
        //{
        //    try
        //    {
        //        jakaAPI.get_robot_state(ref handle2, ref stateR);
        //    }
        //    catch (Exception)
        //    {
        //        return;
        //    }
        //}
        //public static void CX_status_R()
        //{
        //    try
        //    {
        //        jakaAPI.get_robot_status(ref handle2, ref statusR);
        //    }
        //    catch (Exception)
        //    {
        //        return;
        //    }
        //}
        //public static void CX_Stop()
        //{
        //    try
        //    {
        //        jakaAPI.program_abort(ref handle1);
        //    }
        //    catch (Exception)
        //    {
        //        return;
        //    }
        //}
        //public static void CX_Stop_R()
        //{
        //    try
        //    {
        //        jakaAPI.program_abort(ref handle2);
        //    }
        //    catch (Exception)
        //    {
        //        return;
        //    }
        //}
        //public static void Robot_Yun_R()
        //{
        //    try
        //    {
        //        jakaAPI.program_run(ref handle2);
        //    }
        //    catch (Exception)
        //    {
        //        return;
        //    }
        //}
        public static void Read_CSV()//读取轨迹文件
        {
            string line1 = "";
            string line2 = "";
            string line3 = "";
            string line4 = "";
            string line5 = "";
            string line6 = "";
            string path1 = RobotOperateUrl + "\\cup_huibo_swan.csv";
            StreamReader reader1 = new StreamReader(path1);

            line1 = reader1.ReadLine();
            while (line1 != null)
            {
                listStrArr1.Add(line1.Split(','));//将文件内容分割成数组
                line1 = reader1.ReadLine();
            }
            reader1.Close();

            StreamReader reader2 = new StreamReader(RobotOperateUrl + "/milk_huibo_swan.csv");
            line2 = reader2.ReadLine();
            while (line2 != null)
            {
                listStrArr2.Add(line2.Split(','));//将文件内容分割成数组
                line2 = reader2.ReadLine();
            }
            reader2.Close();
            StreamReader reader5 = new StreamReader(RobotOperateUrl + "/cup_huibo_tulip.csv");
            line5 = reader5.ReadLine();
            while (line5 != null)
            {
                listStrArr5.Add(line5.Split(','));//将文件内容分割成数组
                line5 = reader5.ReadLine();
            }
            reader5.Close();
            StreamReader reader6 = new StreamReader(RobotOperateUrl + "/milk_huibo_tulip.csv");
            line6 = reader6.ReadLine();
            while (line6 != null)
            {
                listStrArr6.Add(line6.Split(','));//将文件内容分割成数组
                line6 = reader6.ReadLine();
            }
            reader6.Close();
        }
        public void Robot_Initialisation_double()//机器人初始化
        {
            IOMsg.Instance.AllBol = false;
            Console.WriteLine(DigitalMsg.Instance.RobotPowerOn_start);
            Thread.Sleep(200);
            IOSerialPort.Instance.SerialSend(DigitalMsg.Instance.RobotPowerOn_start);
            Thread.Sleep(3000);
            IOSerialPort.Instance.SerialSend(DigitalMsg.Instance.RobotPowerOn2_start);
            Thread.Sleep(500);
            int tool_idset1 = 0;
            int frame_idset1 = 0;
            int tool_idset2 = 0;
            int frame_idset2 = 0;
            int result1 = -1;
            int result2 = -1;
            while (result1 == -1)
            {
                result1 = jakaAPI.create_handler(IPorPortInfo.Instance.RobotIPLeft.ToCharArray(), ref handle1);
                Thread.Sleep(100);
            }
            while (result2 == -1)
            {
                result2 = jakaAPI.create_handler(IPorPortInfo.Instance.RobotIPRight.ToCharArray(), ref handle2);
                Thread.Sleep(100);
            }
            IOMsg.Instance.AllBol = false;
            Thread.Sleep(100);
            IOSerialPort.Instance.SerialSend(DigitalMsg.Instance.RobotPowerOn_end);
            Thread.Sleep(100);
            IOSerialPort.Instance.SerialSend(DigitalMsg.Instance.RobotPowerOn2_end);
            Thread.Sleep(100);
            IOMsg.Instance.AllBol = true;
            Thread.Sleep(3000); //毫秒
            //机器人上电
            jakaAPI.power_on(ref handle1); Console.WriteLine("L_Robot Power On");
            //机器人上电
            jakaAPI.power_on(ref handle2); Console.WriteLine("R_Robot Power On");
            Thread.Sleep(3000); //毫秒
            #region SDK版本
            ////机器人上电
            //jakaAPI.power_on(ref handle1); Console.WriteLine("L_Robot Power On");
            ////机器人上电
            //jakaAPI.power_on(ref handle2); Console.WriteLine("R_Robot Power On");
            ////切换当前使用的工具坐标
            //jakaAPI.set_tool_id(ref handle1, tool_idset1); Console.WriteLine("L_Robot_T " + tool_idset1);
            ////切换当前使用的世界坐标
            //jakaAPI.set_user_frame_id(ref handle1, frame_idset1); Console.WriteLine("L_Robot_F " + frame_idset1);

            ////切换当前使用的工具坐标
            //jakaAPI.set_tool_id(ref handle2, tool_idset2); Console.WriteLine("R_Robot_T " + tool_idset2);
            ////切换当前使用的世界坐标
            //jakaAPI.set_user_frame_id(ref handle2, frame_idset2); Console.WriteLine("R_Robot_F " + frame_idset2);
            //StringBuilder Version = new StringBuilder("", 3000);
            //int result3 = jakaAPI.get_sdk_version(ref handle1, Version);
            //RobotSerialPortLeft.Instance.Initialization(IPorPortInfo.Instance.RobotIPLeft, IPorPortInfo.Instance.RobotPortLeft);
            //Thread.Sleep(1000);
            #endregion
            #region TCP方式
            Thread thr9 = new Thread(IOMsgR.Instance.IOSerial_Thr);
            thr9.IsBackground = true;
            thr9.Start();
            Thread thr1 = new Thread(IOMsg.Instance.IOSerial_Robot_Thr);
            thr1.IsBackground = true;
            thr1.Start();
            #endregion
            Console.WriteLine("SDK 版本 ");
            Thread.Sleep(2000); //毫秒
            //IOMsg.Instance.AllBol = false;
            //Thread.Sleep(100);
            //IOMsg.Instance.QK();
            //Thread.Sleep(100);
            //IOMsg.Instance.AllBol = true;
            while (!IOMsg.Instance.isRobotBol || !IOMsgR.Instance.isRobotBol) { Thread.Sleep(100); }
            ClientList clientList2 = new ClientList();
            clientList2.MsgBol = WuLiaoCoffeeSel.Instance;
            clientList2.code = 12;
            SocketClients.Instance.client.Send(Encoding.UTF8.GetBytes(JsonConvert.SerializeObject(clientList2)));
            Thread thr2 = new Thread(MainWindow.MessageShow);
            thr2.IsBackground = true;
            thr2.Start();
        }
        /// <summary>
        /// 左臂状态查询
        /// </summary>
        public void Robot_L_Show()
        {
            bool RobotYX = RobotModelLeft.Instance.LRobotJQRYX_Sel();//运行
            bool RobotBJ = RobotModelLeft.Instance.LRobotJQRBJ_Sel();//报警
            bool RobotKJ = RobotModelLeft.Instance.LRobotJQRYKJ_Sel();//开机
            bool RobotSD = RobotModelLeft.Instance.LRobotJQRYSD_Sel();//上电
            bool RobotSN = RobotModelLeft.Instance.LRobotJQRYSN_Sel();//使能
            bool RobotZT = RobotModelLeft.Instance.LRobotJQRZT_Sel();//暂停
            bool RobotYD = RobotModelLeft.Instance.LRobotDYD_Sel();//运动
            if (RobotSD)
                RobotMsgs(false, true, false, false, false, false, false);
            if (RobotSN)
                RobotMsgs(false, false, true, false, false, false, false);
            if (RobotYX)
                RobotMsgs(false, false, false, true, false, false, false);
            if (RobotZT)
                RobotMsgs(false, false, false, false, false, true, false);
            if (!RobotSN)
                RobotMsgs(false, true, false, false, false, false, false);
            if (!RobotSD)
                RobotMsgs(true, false, false, false, false, false, false);
            if (RobotBJ)
                RobotMsgs(false, false, false, false, false, false, true);
        }
        #region 右臂查询
        /// <summary>
        /// 右臂状态查询
        /// </summary>
        public void Robot_R_Show()
        {
            bool RobotYX = RobotModelRight.Instance.RRobotJQRYX_Sel();//运行
            bool RobotBJ = RobotModelRight.Instance.RRobotJQRBJ_Sel();//报警
            bool RobotKJ = RobotModelRight.Instance.RRobotJQRYKJ_Sel();//开机
            bool RobotSD = RobotModelRight.Instance.RRobotJQRYSD_Sel();//上电
            bool RobotSN = RobotModelRight.Instance.RRobotJQRYSN_Sel();//使能
            bool RobotZT = RobotModelRight.Instance.RRobotJQRZT_Sel();//暂停
            bool RobotYD = RobotModelRight.Instance.RRobotJQRYDZ_Sel();//运动
            if (RobotSD)
                RobotMsgs(false, true, false, false, false, false, false, 1);
            if (RobotSN)
                RobotMsgs(false, false, true, false, false, false, false, 1);
            if (RobotYX)
                RobotMsgs(false, false, false, true, false, false, false, 1);
            if (RobotZT)
                RobotMsgs(false, false, false, false, false, true, false, 1);
            if (!RobotSN)
                RobotMsgs(false, true, false, false, false, false, false, 1);
            if (!RobotSD)
                RobotMsgs(true, false, false, false, false, false, false, 1);
            if (RobotBJ)
                RobotMsgs(false, false, false, false, false, false, true, 1);
        }
        #endregion
        /// <summary>
        /// 机器人状态
        /// </summary>
        /// <param name="KJ">开机</param>
        /// <param name="SD">上电</param>
        /// <param name="SN">使能</param>
        /// <param name="YX">运行</param>
        /// <param name="TZ">停止</param>
        /// <param name="ZT">暂停</param>
        /// <param name="BJ">报警</param>
        ///  <param name="type">左右手 0 左 1 右</param>
        public void RobotMsgs(bool KJ, bool SD, bool SN, bool YX, bool TZ, bool ZT, bool BJ, int type = 0)
        {
            if (type == 0)
            {
                RobotMsg.Instance.Robot_YKJ_L = KJ;
                RobotMsg.Instance.Robot_SD_L = SD;
                RobotMsg.Instance.Robot_SN_L = SN;
                RobotMsg.Instance.Robot_YX_L = YX;
                RobotMsg.Instance.Robot_TZ_L = TZ;
                RobotMsg.Instance.Robot_ZT_L = ZT;
                RobotMsg.Instance.Robot_BJ_L = BJ;
            }
            else
            {
                RobotMsg.Instance.Robot_YKJ_R = KJ;
                RobotMsg.Instance.Robot_SD_R = SD;
                RobotMsg.Instance.Robot_SN_R = SN;
                RobotMsg.Instance.Robot_YX_R = YX;
                RobotMsg.Instance.Robot_TZ_R = TZ;
                RobotMsg.Instance.Robot_ZT_R = ZT;
                RobotMsg.Instance.Robot_BJ_R = BJ;
            }
        }
        public void Robot_Initial_Point()//通过关节直线到初始点
        {
            //定义并初始化关JointValue变量
            JKTYPE.JointValue joint_pos1 = new JKTYPE.JointValue();
            JKTYPE.JointValue joint_pos11 = new JKTYPE.JointValue();
            JKTYPE.JointValue joint_pos2 = new JKTYPE.JointValue();
            JKTYPE.JointValue joint_pos22 = new JKTYPE.JointValue();
            //yjx_1J_412 yjx_2J_412
            joint_pos11.jVal = new double[] { 262.227 / 57.3, -43.297 / 57.3, -104.307 / 57.3, 121.504 / 57.3, -73.785 / 57.3, 117.196 / 57.3 };//初始
            joint_pos1.jVal = new double[] { 7.160 / 57.3, 27.992 / 57.3, 119.469 / 57.3, -142.118 / 57.3, 62.838 / 57.3, 159.148 / 57.3 };
            joint_pos2.jVal = new double[] { 292.580 / 57.3, -15.393 / 57.3, -122.816 / 57.3, 110.422 / 57.3, -66.025 / 57.3, 136.822 / 57.3 };
            joint_pos22.jVal = new double[] { 48.915 / 57.3, 29.870 / 57.3, 117.332 / 57.3, -161.063 / 57.3, 58.664 / 57.3, 169.889 / 57.3 };
            Console.WriteLine("joint_move_开始");
            //阻塞运动到目标点位
            jakaAPI.joint_move(ref handle1, ref joint_pos22, JKTYPE.MoveMode.ABS, true, 1);
            jakaAPI.joint_move(ref handle1, ref joint_pos1, JKTYPE.MoveMode.ABS, true, 1);
            jakaAPI.joint_move(ref handle2, ref joint_pos11, JKTYPE.MoveMode.ABS, true, 1);
            jakaAPI.joint_move(ref handle2, ref joint_pos2, JKTYPE.MoveMode.ABS, true, 1);
            //System.Threading.Thread.Sleep(1500); //毫秒
            Console.WriteLine("joint_move_结束");
            //linear_move指令执行开始、
            Console.WriteLine("linear_move_开始");
            JKTYPE.CartesianPose tcp_pos1 = new JKTYPE.CartesianPose();
            JKTYPE.CartesianPose tcp_pos2 = new JKTYPE.CartesianPose();
            //yjx_1J_412   yjx_2J_412
            tcp_pos1.tran.x = 37.274; tcp_pos1.tran.y = 352.718; tcp_pos1.tran.z = 192.557;
            tcp_pos1.rpy.rx = 104.960 / 57.3; tcp_pos1.rpy.ry = -88.416 / 57.3; tcp_pos1.rpy.rz = 139.090 / 57.3;
            tcp_pos2.tran.x = -205.981; tcp_pos2.tran.y = -226.677; tcp_pos2.tran.z = 217.933;
            tcp_pos2.rpy.rx = -95.242 / 57.3; tcp_pos2.rpy.ry = 7.952 / 57.3; tcp_pos2.rpy.rz = 171.149 / 57.3;

            jakaAPI.linear_move(ref handle1, ref tcp_pos1, JKTYPE.MoveMode.ABS, true, 30);
            jakaAPI.linear_move(ref handle2, ref tcp_pos2, JKTYPE.MoveMode.ABS, true, 30);
            Console.WriteLine("linear_move_结束");
        }
        /// <summary>
        /// 天鹅2-机器人伺服关节空间运动
        /// </summary>
        public static void Robot_servo_j_Swan2()//天鹅2-机器人伺服关节空间运动
        {
            Robot_Filter_LPF(3, 10);
            JKTYPE.JointValue[] joint_pos1 = new JKTYPE.JointValue[5000];
            JKTYPE.JointValue[] joint_pos2 = new JKTYPE.JointValue[5000];

            int length = listStrArr1.Count;

            Stopwatch sw = new Stopwatch();//计时器

            for (int i = 0; i < length; i++)
            {
                double[] jointTarget1 = new double[6];
                double[] jointTarget2 = new double[6];

                for (int j = 0; j < 6; j++)
                {
                    jointTarget1[j] = Convert.ToDouble(listStrArr1[i][j]);
                    jointTarget2[j] = Convert.ToDouble(listStrArr2[i][j]);
                    //jakaAPI.servo_j(ref handle1, ref joint_pos1, JKTYPE.MoveMode.ABS);
                }
                joint_pos1[i].jVal = jointTarget1;
                joint_pos2[i].jVal = jointTarget2;
            }
            Console.WriteLine("joint_move");
            jakaAPI.joint_move(ref handle1, ref joint_pos1[0], JKTYPE.MoveMode.ABS, false, 30);
            jakaAPI.joint_move(ref handle2, ref joint_pos2[0], JKTYPE.MoveMode.ABS, true, 30);

            //TRUE为进入伺服模式
            jakaAPI.servo_move_enable(ref handle1, true);
            jakaAPI.servo_move_enable(ref handle2, true);
            //定义并初始化关CartesianPose变量
            System.Threading.Thread.Sleep(300); //毫秒

            Console.WriteLine("servo_j_开始");
            sw.Start();

            for (int i = 0; i < length; i++)
            {
                jakaAPI.servo_j(ref handle1, ref joint_pos1[i], JKTYPE.MoveMode.ABS);
                jakaAPI.servo_j(ref handle2, ref joint_pos2[i], JKTYPE.MoveMode.ABS);
                //jakaAPI.servo_j_extend(ref handle1, ref joint_pos1[i], JKTYPE.MoveMode.ABS,3);
            }
            System.Threading.Thread.Sleep(200);

            //FALSE为退出伺服模式
            jakaAPI.servo_move_enable(ref handle1, false);
            jakaAPI.servo_move_enable(ref handle2, false);
            sw.Stop();
            Console.WriteLine("servo_j_结束{0}s", sw.ElapsedTicks / (decimal)Stopwatch.Frequency);
        }
        /// <summary>
        /// 叶子
        /// </summary>
        public static void Robot_servo_j_Leaf()//树叶-机器人伺服关节空间运动
        {
            Robot_Filter_LPF(0.5, 10);//滤波器设置
            JKTYPE.JointValue[] joint_pos1 = new JKTYPE.JointValue[5000];
            JKTYPE.JointValue[] joint_pos2 = new JKTYPE.JointValue[5000];

            int length = listStrArr3.Count;

            Stopwatch sw = new Stopwatch();//计时器

            for (int i = 0; i < length; i++)
            {
                double[] jointTarget1 = new double[6];
                double[] jointTarget2 = new double[6];

                for (int j = 0; j < 6; j++)
                {
                    jointTarget1[j] = Convert.ToDouble(listStrArr3[i][j]);
                    jointTarget2[j] = Convert.ToDouble(listStrArr4[i][j]);
                    //jakaAPI.servo_j(ref handle1, ref joint_pos1, JKTYPE.MoveMode.ABS);
                }
                joint_pos1[i].jVal = jointTarget1;
                joint_pos2[i].jVal = jointTarget2;
            }
            Console.WriteLine("joint_move");
            jakaAPI.joint_move(ref handle1, ref joint_pos1[0], JKTYPE.MoveMode.ABS, false, 30);
            jakaAPI.joint_move(ref handle2, ref joint_pos2[0], JKTYPE.MoveMode.ABS, true, 30);

            //TRUE为进入伺服模式
            jakaAPI.servo_move_enable(ref handle1, true);
            jakaAPI.servo_move_enable(ref handle2, true);
            //定义并初始化关CartesianPose变量
            System.Threading.Thread.Sleep(300); //毫秒

            Console.WriteLine("servo_j_开始");
            sw.Start();

            for (int i = 0; i < length; i++)
            {
                jakaAPI.servo_j(ref handle1, ref joint_pos1[i], JKTYPE.MoveMode.ABS);
                jakaAPI.servo_j(ref handle2, ref joint_pos2[i], JKTYPE.MoveMode.ABS);
                //jakaAPI.servo_j_extend(ref handle1, ref joint_pos1[i], JKTYPE.MoveMode.ABS,3);
            }
            System.Threading.Thread.Sleep(200);

            //FALSE为退出伺服模式
            jakaAPI.servo_move_enable(ref handle1, false);
            jakaAPI.servo_move_enable(ref handle2, false);
            sw.Stop();
            Console.WriteLine("servo_j_结束{0}s", sw.ElapsedTicks / (decimal)Stopwatch.Frequency);
        }
        public static void Robot_servo_j_Tulip()//郁金香2-机器人伺服关节空间运动
        {
            Robot_Filter_LPF(10, 10);//滤波器设置
            JKTYPE.JointValue[] joint_pos1 = new JKTYPE.JointValue[5000];
            JKTYPE.JointValue[] joint_pos2 = new JKTYPE.JointValue[5000];

            int length = listStrArr5.Count;

            Stopwatch sw = new Stopwatch();//计时器

            for (int i = 0; i < length; i++)
            {
                double[] jointTarget1 = new double[6];
                double[] jointTarget2 = new double[6];

                for (int j = 0; j < 6; j++)
                {
                    jointTarget1[j] = Convert.ToDouble(listStrArr5[i][j]);
                    jointTarget2[j] = Convert.ToDouble(listStrArr6[i][j]);
                    //jakaAPI.servo_j(ref handle1, ref joint_pos1, JKTYPE.MoveMode.ABS);
                }
                joint_pos1[i].jVal = jointTarget1;
                joint_pos2[i].jVal = jointTarget2;
            }
            Console.WriteLine("joint_move");
            jakaAPI.joint_move(ref handle1, ref joint_pos1[0], JKTYPE.MoveMode.ABS, false, 30);
            jakaAPI.joint_move(ref handle2, ref joint_pos2[0], JKTYPE.MoveMode.ABS, true, 30);

            //TRUE为进入伺服模式
            jakaAPI.servo_move_enable(ref handle1, true);
            jakaAPI.servo_move_enable(ref handle2, true);
            //定义并初始化关CartesianPose变量
            System.Threading.Thread.Sleep(300); //毫秒

            Console.WriteLine("servo_j_开始");
            sw.Start();

            for (int i = 0; i < length; i++)
            {
                jakaAPI.servo_j(ref handle1, ref joint_pos1[i], JKTYPE.MoveMode.ABS);
                jakaAPI.servo_j(ref handle2, ref joint_pos2[i], JKTYPE.MoveMode.ABS);
                //jakaAPI.servo_j_extend(ref handle1, ref joint_pos1[i], JKTYPE.MoveMode.ABS,3);
            }
            System.Threading.Thread.Sleep(200);

            //FALSE为退出伺服模式
            jakaAPI.servo_move_enable(ref handle1, false);
            jakaAPI.servo_move_enable(ref handle2, false);
            sw.Stop();
            Console.WriteLine("servo_j_结束{0}s", sw.ElapsedTicks / (decimal)Stopwatch.Frequency);
        }
        public static void Robot_servo_j()//机器人关节空间运动//这个是跑关节的   得读取关节文件
        {
            int ret1; int ret2;
            Stopwatch sw = new Stopwatch();//计时器
            //禁用滤波器
            ret1 = jakaAPI.servo_move_use_none_filter(ref handle1);
            ret2 = jakaAPI.servo_move_use_none_filter(ref handle2);
            //笛卡尔空间滤波选择
            int buffsize = 4;
            double kp = 1;
            double kv = 1;
            double ka = 1;
            const int type = 4;//改这个值选择滤波器1-4
            //禁用滤波器
            ret1 = jakaAPI.servo_move_use_none_filter(ref handle1);
            ret2 = jakaAPI.servo_move_use_none_filter(ref handle2);
            if (type == 1)
            {
                //速度前看
                ret1 = jakaAPI.servo_speed_foresight(ref handle1, buffsize, kp);//Left
                ret2 = jakaAPI.servo_speed_foresight(ref handle2, buffsize, kp);//Right
            }
            else if (type == 2)
            {
                //高阶低通
                ret1 = jakaAPI.servo_move_use_joint_MMF(ref handle1, buffsize, kp, kv, ka);//Left
                ret2 = jakaAPI.servo_move_use_joint_MMF(ref handle2, buffsize, kp, kv, ka);//Right
            }
            else if (type == 3)//非线性
            {
                ret1 = jakaAPI.servo_move_use_carte_NLF(ref handle1, 300, 300, 300, 200, 200, 200);//v a aa w              
                ret2 = jakaAPI.servo_move_use_carte_NLF(ref handle2, 400, 500, 500, 200, 200, 200);
            }
            else if (type == 4)//一阶低通
            {
                ret1 = jakaAPI.servo_move_use_joint_LPF(ref handle1, 0.5);
                ret2 = jakaAPI.servo_move_use_joint_LPF(ref handle2, 5);
            }
            //TRUE为进入伺服模式
            jakaAPI.servo_move_enable(ref handle1, true);
            jakaAPI.servo_move_enable(ref handle2, true);
            //定义并初始化关CartesianPose变量
            System.Threading.Thread.Sleep(500); //毫秒
            JKTYPE.JointValue cart1 = new JKTYPE.JointValue();
            JKTYPE.JointValue cart2 = new JKTYPE.JointValue();
            Console.WriteLine("servo_p_开始");
            sw.Start();
            for (int i = 0; i < 4200; i++)
            {
                //Console.WriteLine(i);
                double Lj1 = Convert.ToDouble(listStrArr1[i][0]); double Lj2 = Convert.ToDouble(listStrArr1[i][1]); double Lj3 = Convert.ToDouble(listStrArr1[i][2]);
                double Lj4 = Convert.ToDouble(listStrArr1[i][3]); double Lj5 = Convert.ToDouble(listStrArr1[i][4]); double Lj6 = Convert.ToDouble(listStrArr1[i][5]);
                double Rj1 = Convert.ToDouble(listStrArr2[i][0]); double Rj2 = Convert.ToDouble(listStrArr2[i][1]); double Rj3 = Convert.ToDouble(listStrArr2[i][2]);
                double Rj4 = Convert.ToDouble(listStrArr2[i][3]); double Rj5 = Convert.ToDouble(listStrArr2[i][4]); double Rj6 = Convert.ToDouble(listStrArr2[i][5]);
                cart1.jVal = new double[] { Lj1, Lj2, Lj3, Lj4, Lj5, Lj6 };
                cart2.jVal = new double[] { Rj1, Rj2, Rj3, Rj4, Rj5, Rj6 };
                //笛卡尔空间伺服运动，其中INCR代表增量运动,ABS代表绝对运动
                jakaAPI.servo_j(ref handle1, ref cart1, JKTYPE.MoveMode.ABS);
                jakaAPI.servo_j(ref handle2, ref cart2, JKTYPE.MoveMode.ABS);
                //System.Threading.Thread.Sleep(1);
            }
            System.Threading.Thread.Sleep(1000);
            //FALSE为退出伺服模式
            jakaAPI.servo_move_enable(ref handle1, false);
            jakaAPI.servo_move_enable(ref handle2, false);
            sw.Stop();
            Console.WriteLine("servo_p_结束{0}s", sw.ElapsedTicks / (decimal)Stopwatch.Frequency);
        }
        public static void Robot_Move(int Action)//机器人运动
        {
            if (Action == 10)//右臂拉花静谧天鹅
            {
                robotMoveC();
                Robot_servo_j_Swan2();
            }
            else if (Action == 22)//右臂拉花郁金秋
            {
                robotMoveC();
                Robot_servo_j_Tulip();
            }
        }
        public static void robotMoveC()
        {
            JKTYPE.OptionalCond opt = new JKTYPE.OptionalCond();
            JKTYPE.CartesianPose end_p = new JKTYPE.CartesianPose(), mid_p = new JKTYPE.CartesianPose();
            JKTYPE.CartesianPose end_p1 = new JKTYPE.CartesianPose(), mid_p1 = new JKTYPE.CartesianPose();

            end_p.tran.x = 210; end_p.tran.y = 279; end_p.tran.z = 243;
            end_p.rpy.rx = -90 / 57.3; end_p.rpy.ry = -0 / 57.3; end_p.rpy.rz = 0 / 57.3;
            mid_p.tran.x = 225; mid_p.tran.y = 264; mid_p.tran.z = 243;
            mid_p.rpy.rx = -90 / 57.3; mid_p.rpy.ry = -0 / 57.3; mid_p.rpy.rz = 0 / 57.3;

            end_p1.tran.x = 210; end_p1.tran.y = 249; end_p1.tran.z = 243;
            end_p1.rpy.rx = -90 / 57.3; end_p1.rpy.ry = -0 / 57.3; end_p1.rpy.rz = 0 / 57.3;
            mid_p1.tran.x = 195; mid_p1.tran.y = 264; mid_p1.tran.z = 243;
            mid_p1.rpy.rx = -90 / 57.3; mid_p1.rpy.ry = -0 / 57.3; mid_p1.rpy.rz = 0 / 57.3;

            JKTYPE.JointValue joint_pos = new JKTYPE.JointValue();
            joint_pos.jVal = new double[] { 24.716 / 57.3, -19.678 / 57.3, -112.003 / 57.3, 107.019 / 57.3, -71.803 / 57.3, 135.574 / 57.3 };


            JKTYPE.JointValue joint_pos1 = new JKTYPE.JointValue();
            joint_pos1.jVal = new double[] { 24.716 / 57.3, -19.678 / 57.3, -112.003 / 57.3, 107.019 / 57.3, -71.803 / 57.3, 135.574 / 57.3 };
            jakaAPI.joint_move(ref handle2, ref joint_pos1, JKTYPE.MoveMode.ABS, true, 1);//A点

            Console.WriteLine("move_j 开始");
            jakaAPI.joint_move(ref handle2, ref joint_pos1, JKTYPE.MoveMode.ABS, true, 100);
            Console.WriteLine("circle_move_开始");

            //jakaAPI.circular_move(ref handle1, ref end_p, ref mid_p, JKTYPE.MoveMode.ABS, false, 170, 50, 5, ref opt);
            //jakaAPI.circular_move(ref handle1, ref end_p1, ref mid_p1, JKTYPE.MoveMode.ABS, false, 170, 50, 5, ref opt);

            double speed = 200;
            double accel = 100;

            for (int i = 0; i < 15; i++)
            {
                jakaAPI.circular_move(ref handle2, ref end_p, ref mid_p, JKTYPE.MoveMode.ABS, false, speed, accel, 5, ref opt);
                jakaAPI.circular_move(ref handle2, ref end_p1, ref mid_p1, JKTYPE.MoveMode.ABS, false, speed, accel, 5, ref opt);
            }
            jakaAPI.circular_move(ref handle2, ref end_p, ref mid_p, JKTYPE.MoveMode.ABS, false, speed, accel, 5, ref opt);
            jakaAPI.circular_move(ref handle2, ref end_p1, ref mid_p1, JKTYPE.MoveMode.ABS, true, speed, accel, 0, ref opt);

            Console.WriteLine("circle_move_结束");
        }

        static void Robot_FilterSetting(int type, double lv, double kp = 1, double kp2 = 1)//机器人滤波器控制
        {
            int ret1;
            int ret2;

            int buffsize = 4;
            // double kp = 1;

            double kv = 1;
            double ka = 1;

            //const int type = 3;//1速度前看和3一阶低通效果较好

            //禁用滤波器
            ret1 = jakaAPI.servo_move_use_none_filter(ref handle1);
            ret2 = jakaAPI.servo_move_use_none_filter(ref handle2);
            //System.Threading.Thread.Sleep(500);

            if (type == 1)
            {
                //速度前看
                ret1 = jakaAPI.servo_speed_foresight(ref handle1, buffsize, kp);//Left
                ret2 = jakaAPI.servo_speed_foresight(ref handle2, buffsize, kp2);                                                               // ret2 = jakaAPI.servo_speed_foresight(ref handle2, buffsize, kp);//Right
            }
            else if (type == 2)
            {
                //高阶低通
                ret1 = jakaAPI.servo_move_use_joint_MMF(ref handle1, buffsize, kp, kv, ka);//Left
                ret2 = jakaAPI.servo_move_use_joint_MMF(ref handle2, buffsize, kp, kv, ka);//Right
            }
            else if (type == 3)
            {
                //一阶低通
                ret1 = jakaAPI.servo_move_use_joint_LPF(ref handle1, 3);
                ret2 = jakaAPI.servo_move_use_joint_LPF(ref handle2, lv);

            }
            else if (type == 4)
            {
                //非线性滤波
                ret1 = jakaAPI.servo_move_use_joint_NLF(ref handle1, 40, 40, 80);
                ret2 = jakaAPI.servo_move_use_joint_NLF(ref handle2, 40, 40, 80);
            }
            //System.Threading.Thread.Sleep(500);

        }
        static void Robot_Filter_LPF(double lv1, double lv2)//机器人LPF滤波器控制
        {


            //禁用滤波器
            int ret1 = jakaAPI.servo_move_use_none_filter(ref handle1);
            int ret2 = jakaAPI.servo_move_use_none_filter(ref handle2);

            //一阶低通
            int ret11 = jakaAPI.servo_move_use_joint_LPF(ref handle1, lv1);
            int ret22 = jakaAPI.servo_move_use_joint_LPF(ref handle2, lv2);



        }
    }
}
